package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.model.MetroLine;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S1: minimizes the spline curvature
 */
public class ForceS1 extends ForceSubsystem {
	
	ForceS1(ForceSystem system) {
		super(system);
	}
	
	void applyForces(int delta) {
		for (MetroLine l : system.metroMap.lines) {
			for (int i = 1; i < l.stations.length - 1; i++) {
				MetroStation m1 = l.stations[i - 1]; // left child
				MetroStation m2 = l.stations[i];     // middle / root
				MetroStation m3 = l.stations[i + 1]; // right child
				Body b1 = system.stationBodies[m1.id];
				Body b2 = system.stationBodies[m2.id];
				Body b3 = system.stationBodies[m3.id];
				
				Vec2 p1 = system.stationBodies[m1.id].getPosition(); 
				Vec2 p2 = system.stationBodies[m2.id].getPosition(); 
				Vec2 p3 = system.stationBodies[m3.id].getPosition();
				
				Vec2 p21 = p1.sub(p2);
				Vec2 p23 = p3.sub(p2);
				
				double theta21 = Math.atan2(p21.y, p21.x); // between p21 and x-axis
				double theta23 = Math.atan2(p23.y, p23.x); // between p23 and x-axis
				
				// First rotate the vectors such that the vector from 2 to 1
				// lies on the x-axis. Then subtract Pi to set a straight line
				// (opposite vectors) to 0.
				double theta = (theta23 - theta21) - Math.PI - 0.233f;
				// Move the range to [-pi, pi).
				if (theta <= -Math.PI)
					theta += 2f * Math.PI;
				
				Vec2 f1 = new Vec2(-p21.y,  p21.x);
				Vec2 f3 = new Vec2( p23.y, -p23.x);
				
				float len1 = f1.normalize();
				float len3 = f3.normalize();

				f1.mulLocal((float)theta * ForceConstants.S1_WEIGHT / len1);
				f3.mulLocal((float)theta * ForceConstants.S1_WEIGHT / len3);

				b1.applyForceToCenter(f1);
				b3.applyForceToCenter(f3);
				
				Vec2 f2 = f1.negateLocal().add(f3.negateLocal());
				b2.applyForceToCenter(f2);
			}
		}		
	}
}
